import json
import re
import socket
import threading
from dronekit import connect, VehicleMode, LocationGlobal
import time
from pymavlink import mavutil

# 改为当前连接的飞控的端口
connection_string = '10.42.0.10:14550'
print('Connecting to vehicle on: %s' % connection_string)
# connect函数将会返回一个Vehicle类型的对象，即此处的vehicle
# 即可认为是无人机的主体，通过vehicle对象，我们可以直接控制无人机
vehicle = connect('/dev/ttyUSB0', wait_ready=True, baud=921600)


# 高度以米为单位
def arm_and_takeoff(aTargetAltitude):
    """
    Arms vehicle and fly to aTargetAltitude.
    """
    #  进行起飞前检查
    print("Basic pre-arm checks")
    # vehicle.is_armable会检查飞控是否启动完成、有无GPS fix、卡曼滤波器
    # 是否初始化完毕。若以上检查通过，则会返回True
    # while not vehicle.is_armable:
    #     print(" Waiting for vehicle to initialise...")
    #     time.sleep(1)

    # 解锁无人机（电机将开始旋转）
    print("Arming motors")
    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机）
    vehicle.mode = VehicleMode("GUIDED")
    # 通过设置vehicle.armed状态变量为True，解锁无人机
    vehicle.armed = True

    # 在无人机起飞之前，确认电机已经解锁
    while not vehicle.armed:
        print(" Waiting for arming...")
        time.sleep(1)

    # 发送起飞指令
    print("Taking off!")
    # simple_takeoff将发送指令，使无人机起飞并上升到目标高度
    # send_takeoff_command(aTargetAltitude)
    vehicle.simple_takeoff(aTargetAltitude)
    #
    # # 在无人机上升到目标高度之前，阻塞程序
    while True:
        print(" Altitude: ", vehicle.location.global_relative_frame.alt)
        # 当高度上升到目标高度的0.95倍时，即认为达到了目标高度，退出循环
        # vehicle.location.global_relative_frame.alt为相对于home点的高度
        if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95:
            print("Reached target altitude")
            break
            # 等待1s
        time.sleep(1)


# 降落
def land():
    vehicle.mode = VehicleMode("LAND")


# 飞行方向速度 * 时间 = 距离
# x表示前后方向,y表示左右方向,z表示垂直高度方向
def send_nav_velocity(velocity_x, velocity_y, velocity_z, duration):
    # 生成SET_POSITION_TARGET_LOCAL_NED命令
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,  # time_boot_ms (not used)
        0, 0,  # target system, target component
        mavutil.mavlink.MAV_FRAME_BODY_NED,  # frame
        0b0000111111000111,  # type_mask (only speeds enabled)
        0, 0, 0,  # x, y, z positions (not used)
        velocity_x, velocity_y, velocity_z,  # x, y, z velocity in m/s
        0, 0, 0,  # x, y, z acceleration (not used)
        0, 0)  # yaw, yaw_rate (not used)
    for x in range(0, duration):
        vehicle.send_mavlink(msg)
        time.sleep(1)


def condition_yaw(heading, relative=True, clock_wise=True):
    """
    设置无人机的朝向角。

    :param heading: 期望的航向角度。
    :param relative: 布尔值，决定是使用相对角度还是绝对角度。
    :param clock_wise: 布尔值，决定转动方向是顺时针还是逆时针。
    """

    # 初始化变量，用于决定是否使用相对角度
    # 如果 relative 为 True，则 isRelative 设为 1，表示使用相对角度
    # 否则设为 0，表示使用绝对角度
    isRelative = 1 if relative else 0

    # 初始化变量，用于决定转动方向
    # 如果使用相对角度，并且 clock_wise 为 True，则 direction 设为 1，表示顺时针转动
    # 如果使用相对角度，并且 clock_wise 为 False，则 direction 设为 -1，表示逆时针转动
    # 如果不使用相对角度，则 direction 设为 0，表示不使用相对角度转动
    direction = 1 if clock_wise else -1
    if not relative:
        direction = 0

    # 生成 MAVLink CONDITION_YAW 命令
    # 该命令用于设置无人机的朝向角
    msg = vehicle.message_factory.command_long_encode(
        0, 0,  # target system, target component
        # 通常设置为 0，除非有多个系统或组件
        mavutil.mavlink.MAV_CMD_CONDITION_YAW,  # 命令类型，这里是设置朝向角
        0,  # 确认参数，通常设置为 0
        heading,  # 参数 1，期望的航向角度（单位：度）
        0,  # 参数 2，期望的航向角速度（未使用，通常设置为 0）
        direction,  # 参数 3，转动方向（1: 顺时针, -1: 逆时针, 0: 绝对角度）
        isRelative,  # 参数 4，决定是否使用相对角度（1: 相对, 0: 绝对）
        0, 0, 0  # 参数 5-7，未使用，通常设置为 0
    )

    # 发送 MAVLink 命令到无人机
    vehicle.send_mavlink(msg)
    # 确保所有挂起的 MAVLink 消息被发送出去
    vehicle.flush()


def toLeftSingle():
    print("往左一格")
    send_nav_velocity(0, -0.3, 0, 2)
    send_nav_velocity(0, 0, 0, 1)


def toRightSingle():
    print("往右一格")
    send_nav_velocity(0, 0.3, 0, 2)
    send_nav_velocity(0, 0, 0, 2)


def toForwardSingle():
    print("往前一格")
    send_nav_velocity(0.3, 0, 0, 2)
    send_nav_velocity(0, 0, 0, 2)


def toBackSingle():
    print("往后一格")
    send_nav_velocity(-0.3, 0, 0, 2)
    send_nav_velocity(0, 0, 0, 2)


def avoidObstacle(direction):
    # 根据避障方向执行避障动作
    if direction == 1:
        print("执行避障：向左")
        toLeftSingle()
        toLeftSingle()
        toForwardSingle()
    elif direction == 2:
        print("执行避障：向右")
        toRightSingle()
        toForwardSingle()


isExist = False  # 是否存在障碍物
direction = 1  # 穿过障碍物的方向 1 左  2 右
lastX = 7  # 穿过障碍物后 下次航点开始的X
lastY = 6  # 穿过障碍物后 下次航点开始的Y
headToward = 1  # 机头朝向 1 朝前  -1 朝后


# 启用线程一直接收 是否有避障指令
def isExistObstacle():
    global isExist
    global direction
    isExist = False
    # 创建UDP套接字并绑定到指定的端口
    udp_socket1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定UDP套接字到 1000
    udp_socket1.bind(('10.42.0.10', 10000))
    print(f"UDP server listening on 接收避障指令{('10.42.0.10', 10000)}")
    while True:
        # 接收数据
        data, addr = udp_socket1.recvfrom(1024)
        data_length = len(data)
        print(f"Received data: {data} 长度为:{data_length}")
        command = data.decode('utf-8')
        if command == "左避障":
            isExist = True
            direction = 1
            print("前方障碍物 左避障")
            avoidObstacle(direction)  # 测试
        if command == "右避障":
            isExist = True
            direction = 2
            print("前方障碍物 右避障")
            avoidObstacle(direction)  # 测试


# 定义一个函数来处理接收到的UDP数据
def handle_udp_data():
    # 创建UDP套接字并绑定到指定的端口
    udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定UDP套接字到指定的端口
    udp_socket.bind(('10.42.0.10', 1111))
    print(f"UDP server listening on {('10.42.0.10', 1111)}")
    global lastX  # 穿过障碍物后 下次航点开始的X
    global lastY  # 穿过障碍物后 下次航点开始的Y
    # 初始时为起点位置
    lastX = 7
    lastY = 6
    while True:
        # 接收数据
        data, addr = udp_socket.recvfrom(1024)
        data_length = len(data)
        print(f"Received data: {data} 长度为:{data_length}")
        if data_length > 1:
            # 尝试将数据解析为JSON
            point_data = json.loads(data.decode('utf-8'))
            execute_instructions(point_data)
        else:  # 起飞降落进 1 2
            numbers = re.findall(r'\d+', data.decode('utf-8'))
            if numbers:
                print("数字")
                execute_instructions(data.decode('utf-8'))


# 执行航点任务
def executePoints(points):
    global isExist
    global lastX
    global lastY
    global headToward
    # 实现有头飞行
    # 计算航点与无人机当前位置的差值 并决定飞机转头的角度
    centerX = lastX
    centerY = lastY
    # x为前后 y为左右
    for point in points:
        diffX = point['x'] - centerX
        diffY = point['y'] - centerY
        for i in range(0, abs(diffY)):  # 左右移动 机头不跟随移动
            # 考虑 机头朝前 还是朝后
            if headToward == 1:  # 机头朝前
                if diffY < 0:  # 左
                    toLeftSingle()
                else:
                    toRightSingle()
            if headToward == -1:
                if diffY < 0:
                    toRightSingle()
                else:
                    toLeftSingle()
        for i in range(0, abs(diffX)):  # 前后移动 机头跟随移动
            # 考虑机头转向 180 分两次90 或者 一次性180 ？？待测
            if diffX > 0:
                headToward = -1
            else:
                headToward = 1
            if headToward == -1:
                print("调整机头朝后")
                break  # 中断测试
            if isExist:  # 检查是否有避障指令发送过来
                # 不管哪个方向避障 最后都是位置都是 x y-2
                lastX = point['x'] - i - 2
                lastY = point['y']
                print("避障结束后 (x, y)：" + str(lastX) + "," + str(lastY))
                avoidObstacle(direction)
                isExist = False
                break
            else:  # 转头后 不论方向 都是往前走
                toForwardSingle()
                time.sleep(1)  # 等待1秒检测前方障碍物
        if isExist:
            isExist = False
            break  # 结束本次航点任务
        centerX = point['x']
        centerY = point['y']


# 定义一个函数来按照队列顺序执行动作
# 1 起飞  2  降落
def execute_instructions(instruction):
    flight_direction = []  # 飞行方向
    # 检查指令是否为字典类型 读入航点数据
    # 执行固定航点任务
    if len(instruction) > 1:
        print("执行航点任务")
        print(instruction)
        executePoints(instruction)
    # 执行指令对应的动作
    if instruction == "1":
        print("启动")
        # 默认飞行高度0.6m
        arm_and_takeoff(0.6)
        print(f"Executing instruction: 起飞")
    if instruction == "2":
        land()
        print(f"Executing instruction: 降落")
    if instruction == "3":
        print(f"Executing instruction: 往前一格")
        toForwardSingle()
    if instruction == "4":
        toBackSingle()
        print(f"Executing instruction: 往后一格")
    if instruction == "5":
        # 相对角度  逆时针 旋转90
        condition_yaw(90, True, False)
        time.sleep(1)  # 等待转头完成从
        # print("逆时针转头90")
        toLeftSingle()
        print(f"Executing instruction: 往左一格")
    if instruction == "6":
        # condition_yaw(90, True, True)
        # print("顺时针转头90")
        toRightSingle()
        print(f"Executing instruction: 往右一格")
    time.sleep(0.5)


# 创建一个线程来处理UDP数据
udp_thread = threading.Thread(target=handle_udp_data)
avoidObstacle_thread = threading.Thread(target=isExistObstacle)
avoidObstacle_thread.start()
udp_thread.start()
